#include "M451Series.h"
#include "global.h"

uint16_t gTrapezoid1 = 0;
uint16_t gTrapezoid2 = 0;
uint16_t gTrapezoid3 = 0;
uint16_t gTrapezoid4 = 0;
uint16_t gTrapezoid5 = 0;
uint16_t gTrapezoid6 = 0;

uint32_t gServoMotorAngle  = 3;
void PWM0P0_IRQHandler(void)
{
	  static uint32_t lastStep = 0;
		PWM_EnableOutput(PWM0, PWM_CH_0_MASK);

		if(Axis1 != 0 ){
				if((Axis1 > 0)&&(gServoMotorAngle < 13)){
					gServoMotorAngle ++;
					//PB4 = 1;
				}else if((Axis1 < 0)&&(gServoMotorAngle > 3)){
					gServoMotorAngle--;
					//PB4 = 0;
				}
				PWM_ConfigOutputChannel(PWM0, 0, 50, gServoMotorAngle);  //port 5

		}
		else{
		}
		if(Axis2  != 0){
				PWM_EnableOutput(PWM0, PWM_CH_1_MASK);
				if(Axis2 > 0){
					PB8 = 1;
				}else{
					PB8 = 0;
				}
		}
		else{
				PWM_DisableOutput(PWM0, PWM_CH_1_MASK);
		}
		if(Axis3 != 0){
				if(Axis3 > 0){
					PB9 = 1;
				}else{
					PB9 = 0;
				}			
				PWM_EnableOutput(PWM0, PWM_CH_3_MASK);   //port6
		}
		else{
				PWM_DisableOutput(PWM0, PWM_CH_3_MASK);
		}
		if(Axis4 != 0){
				PWM_EnableOutput(PWM0, PWM_CH_4_MASK);
				if(Axis4 > 0){
					PB11 = 1;
				}else{
					PB11 = 0;
				}					
		}else{
				PWM_DisableOutput(PWM0, PWM_CH_4_MASK);
		}
    // Clear channel 0 period interrupt flag
    PWM_ClearPeriodIntFlag(PWM0, 0);
}

void PWM1P0_IRQHandler(void){
	if(Axis5 != 0){
		PWM_EnableOutput(PWM1, PWM_CH_0_MASK);
		if(Axis5 > 0){
			PB4 = 1;
		}else{
			PB4 = 0;
		}			
	}else{
		PWM_DisableOutput(PWM1, PWM_CH_0_MASK);
	}
	if(Axis6 != 0){
		PWM_EnableOutput(PWM1, PWM_CH_1_MASK);
		if(Axis6 > 0){
			PA9 = 1;
		}else{
			PA9 = 0;
		}			
	}else{
		PWM_DisableOutput(PWM1, PWM_CH_1_MASK);
	}
	PWM_ClearPeriodIntFlag(PWM1, 0);

}
